/* Includes ------------------------------------------------------------------*/
#include "vl53l0x_a.h"
#include "gpio.h"

/*----------------------------------------------------------------------------*/
/* Configure vl53l0x_a                                                        */
/*----------------------------------------------------------------------------*/

uint8_t vl_a_a_slave_addr = 0x52;

void VL53L0X_A_sda_out(uint8_t out) {
  GPIO_InitTypeDef GPIO_InitStruct;

  HAL_GPIO_WritePin(vl_a_GPIO_Port, vl_a_sda_Pin, GPIO_PIN_SET);

  GPIO_InitStruct.Pin = vl_a_sda_Pin;
  if (out) {
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  } else {
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  }

  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(vl_a_GPIO_Port, &GPIO_InitStruct);
}

void VL53L0X_A_IIC_Start(void) {
  VL53L0X_A_SCL_Set();
  VL53L0X_A_SDA_Set();
  VL53L0X_A_SDA_Clr();
}

void VL53L0X_A_IIC_Stop(void) {
  VL53L0X_A_SCL_Set();
  VL53L0X_A_SDA_Clr();
  VL53L0X_A_SDA_Set();
}

void VL53L0X_A_IIC_Wait_Ack(void) {
  VL53L0X_A_SCL_Set();
  VL53L0X_A_SCL_Clr();
}

void VL53L0X_A_IIC_Ack(uint8_t ack) {
  VL53L0X_A_SCL_Clr();

  if (ack) {
    VL53L0X_A_SDA_Clr();
  } else {
    VL53L0X_A_SDA_Set();
  }

  VL53L0X_A_SCL_Set();
  VL53L0X_A_SCL_Clr();
}

void VL53L0X_A_Write_IIC_Byte(uint8_t IIC_Byte) {
  uint8_t i;

  VL53L0X_A_SCL_Clr();

  for (i = 0; i < 8; i++) {
    if (IIC_Byte & 0x80) {
      VL53L0X_A_SDA_Set();
    } else {
      VL53L0X_A_SDA_Clr();
    }
    IIC_Byte <<= 1;
    VL53L0X_A_SCL_Set();
    VL53L0X_A_SCL_Clr();
  }
}

uint8_t VL53L0X_A_Read_IIC_Byte(uint8_t ack) {
  uint8_t i, receive = 0;

  VL53L0X_A_sda_out(0);

  for (i = 0; i < 8; i++) {
    VL53L0X_A_SCL_Clr();
    __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
    VL53L0X_A_SCL_Set();

    receive <<= 1;

    if (VL53L0X_A_READ_SDA) {
      receive++;
    }
  }

  VL53L0X_A_sda_out(1);

  if (ack) {
    VL53L0X_A_IIC_Ack(1);
  } else {
    VL53L0X_A_IIC_Ack(0);
  }

  return receive;
}

void VL53L0X_A_Single_Write(uint8_t addr, uint8_t data) {
  VL53L0X_A_IIC_Start();
  VL53L0X_A_Write_IIC_Byte(vl_a_a_slave_addr);
  VL53L0X_A_IIC_Wait_Ack();
  VL53L0X_A_Write_IIC_Byte(addr);
  VL53L0X_A_IIC_Wait_Ack();
  VL53L0X_A_Write_IIC_Byte(data);
  VL53L0X_A_IIC_Wait_Ack();
  VL53L0X_A_IIC_Stop();
}

uint8_t VL53L0X_A_Single_Read(uint8_t addr) {
  uint8_t rec;

  VL53L0X_A_IIC_Start();
  VL53L0X_A_Write_IIC_Byte(vl_a_a_slave_addr);
  VL53L0X_A_IIC_Wait_Ack();
  VL53L0X_A_Write_IIC_Byte(addr);
  VL53L0X_A_IIC_Wait_Ack();

  VL53L0X_A_IIC_Start();
  VL53L0X_A_Write_IIC_Byte(vl_a_a_slave_addr + 1);
  VL53L0X_A_IIC_Wait_Ack();

  rec = VL53L0X_A_Read_IIC_Byte(0);
  VL53L0X_A_IIC_Stop();

  return rec;
}

uint16_t VL53L0X_A_Distance(void) {
  uint8_t cnt = 0;
  uint8_t val;
  uint16_t distance;

  VL53L0X_A_Single_Write(VL53L0X_A_REG_SYSRANGE_START, 0x01);

  // 1 second waiting time max
  while (cnt < 200) {
    val = VL53L0X_A_Single_Read(VL53L0X_A_REG_RESULT_RANGE_STATUS);
    if (val & 0x01) break;
    cnt++;
    HAL_Delay(5);
  }

  distance = VL53L0X_A_Single_Read(VL53L0X_A_REG_RESULT_RANGE_STATUS + 10);
  distance <<= 8;
  distance |= VL53L0X_A_Single_Read(VL53L0X_A_REG_RESULT_RANGE_STATUS + 11);

  return distance;
}

void VL53L0X_A_Init(void) {
  GPIO_InitTypeDef GPIO_InitStruct;

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOB_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(vl_a_GPIO_Port, vl_a_scl_Pin | vl_a_sda_Pin, GPIO_PIN_SET);

  /*Configure GPIO pins : PBPin PBPin */
  GPIO_InitStruct.Pin = vl_a_scl_Pin | vl_a_sda_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(vl_a_GPIO_Port, &GPIO_InitStruct);
}
